#ifndef _CDISPTRACKER_H_
#define _CDISPTRACKER_H_

//#include <pyra/kalman.h>
#include <buola/image.h>
#include <buola/cv/corners.h>

namespace buola { namespace cvi {

class CDispTracker
{
public:
    CDispTracker();

    void GetParameters(float &pXDif,float &pYDif,float &pZRot,float &pScale);
    CPoint_d GetCenter();
    void Update(const img::CView_gray8 &pLeft,const img::CView_gray8 &pRight,
                float pPanPred=0.0,float pTiltPred=0.0,float pVergPred=0.0);

private:
    int FindSimilarityMatrix();
    int FindRelativePose(int pW,int pH,float &pXDif,float &pYDif,float &pZRot,float &pScale);
/*
    void KalmanUpdate(float xdif,float ydif,float zrot,float scale,float xpos,
                      float ypos,float xpos_center,float ypos_center,
                      float xdif_center,float pxpos,float pypos,float pxdif);
    bool AffineTransfer(CCornerList &pLeft,CCornerList &pRight,int w,int h);
    bool FindAffineTransform(Corners &lcorners,bool applyleft,float *atrans);
    int FindAffineFMatrix(Corners::CVector &ptsl);
    bool FindCenter(Corners &lcorners,int w,int h,float ydif,float &xpos,
                    float &ypos,float &xdif,float prunewei=0.0);
    void DispSegment(Image<float> &dimg,Image<unsigned char> &cut,float dsiz,
                     float drange,float &xpos,float &ypos);
*/

private:
//    static const float mExpYErr=3.0f;
    static constexpr float mMinThresh=5.0f;
    static constexpr float mMaxThresh=35.0f;
    static constexpr int mExpNumPts=500;

//    float mFocal;
//    int mCount; // Update counter
//    int mSubSample; // subsampling
//    bool mWithDisp; // Using disparities for position
//    bool mTracking; // Whether tracking or just bootstrap
//    int mNumValid; // Number of valid features
//    float mExpRange; // Expected object depth range
//    CSize_d mExpSize; // Expected object size
//    Kalman<2> mKalScale,mKalZRot,mKalXDif,mKalYDif,mKalXPos,mKalYPos;
//    Kalman<2> mKalXDifCenter,mKalXPosCenter,mKalYPosCenter;
    float mLeftThresh;
    float mRightThresh;

    float mParams[5]; // Affine fundamental matrix
    float mTrans[8]; // Paraperspective transformation
//    CPoint_d mLeftPos; // Left tracking pos
//    CPoint_d mRightPos; // Right tracking pos
    float mXDif,mYDif,mZRot,mScale;


    CCornerList mLCorners,mRCorners;
    CCornerList mLPrevCorners,mRPrevCorners;
    std::vector<cvi::CCornerMatch> mStereoMatch;
};

/*namespace cvi*/ } /*namespace buola*/ }

#endif // DISPTRACK_H
